#include "include/cycle_task.h"
#include "include/ui_task.h"
#include "include/config_qn.h"
#include "include/check_state.h"
struct timespec wakeup_time;


/****************************************************************************/

void stack_prefault(void)
{
    unsigned char dummy[MAX_SAFE_STACK];

    memset(dummy, 0, MAX_SAFE_STACK);
}

/*****************************************/

void domain_init(){
 //Aix1
 
 EC_WRITE_U16(domainOutput_pd,0x07);
 EC_WRITE_U32(domainOutput_pd+2,A1.actpos);
 EC_WRITE_U8(domainOutput_pd+6,0x08);
 //EC_WRITE_U16(domainOutput_pd+8,0x00);
 //EC_WRITE_U16(domainOutput_pd+10,0x00);
 /****
 //Aix2
 EC_WRITE_U16(domainOutput_pd+8,0x07);
 EC_WRITE_U32(domainOutput_pd+8+2,0x6e1303);
 EC_WRITE_U8(domainOutput_pd+8+6,0x08);
 //EC_WRITE_U16(domainOutput_pd+12+8,0x00);
 //EC_WRITE_U16(domainOutput_pd+12+10,0x00);
 //Aix3
 EC_WRITE_U16(domainOutput_pd+16,0x07);
 EC_WRITE_U32(domainOutput_pd+16+2,0x6e1303);
 EC_WRITE_U8(domainOutput_pd+16+6,0x08);
 //EC_WRITE_U16(domainOutput_pd+24+8,0x00);
// EC_WRITE_U16(domainOutput_pd+24+10,0x00);
 //Aix4
 EC_WRITE_U16(domainOutput_pd+24,0x07);
 EC_WRITE_U32(domainOutput_pd+24+2,0x6e1303);
 EC_WRITE_U8(domainOutput_pd+24+6,0x08);
// EC_WRITE_U16(domainOutput_pd+36+8,0x00);
// EC_WRITE_U16(domainOutput_pd+36+10,0x00);
****/
}



void cyclic_task(){
    //ecrt_master_set_send_interval(master,1000);
    ecrt_master_receive(master); 
    printf("1 ok?\n");   	
    ecrt_domain_process(domainServoInput);
    ecrt_domain_process(domainServoOutput);
    printf("2 ok?\n");
    check_domainInput_state();
    check_domainOutput_state();
    check_master_state();
    check_slave0_config_states();
    //A1_data.status=EC_READ_U32(domainOutput_pd);     
    //printf("A1.status=%d",A1_data.status);
    
    //EC_WRITE_U16(domainInput_pd,0x45);     	
    // send process data
    //domain_init();
    clock_gettime(CLOCK_REALTIME, &tv); 
    // write application time to master
    ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
    ecrt_master_sync_reference_clock(master);   
    ecrt_master_sync_slave_clocks(master);      

    printf("3 ok?\n");
    ecrt_domain_queue(domainServoOutput);
    ecrt_domain_queue(domainServoInput);
    printf("ok?\n");
    pause(); 
    //ecrt_master_set_send_interval(master,1000);
    ecrt_master_send(master);


}



void set_priority(){
 /* Set priority */

    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);

    printf("Using priority %i.", param.sched_priority);
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
        perror("sched_setscheduler failed");
    }

    /* Lock memory */

    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        fprintf(stderr, "Warning: Failed to lock memory: %s\n",
                strerror(errno));
    }

    stack_prefault();

    printf("Starting RT task with dt=%u ns.\n", PERIOD_NS);

    clock_gettime(CLOCK_MONOTONIC, &wakeup_time);
    wakeup_time.tv_sec += 1; /* start in future */
    wakeup_time.tv_nsec = 0;
}

int data_cycle_task(){
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain_in data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain_out data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");
    
    set_priority();
    
    domain_init();
    while(1){
    	printf("start cyclic_task.wait 5s\n");
        //sleep(5);
        cyclic_task();
        ecrt_master_set_send_interval(master,4000);
    }
    /***************
     int ret = 0;
     while (1) {
        ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
                &wakeup_time, NULL);
        if (ret) {
            fprintf(stderr, "clock_nanosleep(): %s\n", strerror(ret));
            break;
        }
        printf("start cyclic_task.wait 5s\n");
        //sleep(5);
        cyclic_task();
        ecrt_master_set_send_interval(master,4000);

        wakeup_time.tv_nsec += PERIOD_NS;
        while (wakeup_time.tv_nsec >= NSEC_PER_SEC) {
            wakeup_time.tv_nsec -= NSEC_PER_SEC;
            wakeup_time.tv_sec++;
        }
    }

    return ret;****/
    
}


int enable_task(){
    A1_data.cntlwd=0xf;
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");
    
    
    ecrt_master_receive(master); 
    printf("1 ok?");   	
    ecrt_domain_process(domainServoInput);
    ecrt_domain_process(domainServoOutput);
    printf("2 ok?");
    check_domainInput_state();
    check_domainOutput_state();
    
    //A1_data.status=EC_READ_U32(domainOutput_pd+A1.status);     
    //printf("A1.status=%d",A1_data.status);
    
    //EC_WRITE_U16(domainInput_pd+A1.cntlwd,A1_data.cntlwd);    
    
    	
    	// send process data
    printf("3 ok?");
    ecrt_domain_queue(domainServoOutput);
    ecrt_domain_queue(domainServoInput);
    printf("ok?");
    pause(); 
    ecrt_master_send(master);
    	
}








